#include <iostream>

#include "determine_drive_command.h"
#include "robot_orientation.h"
#include "blobdetector.h"
#include "facefinder.h"
#include "rovio.h"

//#define ROVIO_HOST "http://143.215.111.166"
#define ROVIO_HOST "http://143.215.111.166"
#define DUMP(x) cout << #x << ": " << x << endl;
#define AT //cout << "At " << index++ << endl

using namespace cv;
using namespace std;

int main(int argc, char **argv) {
	Rovio rovio(ROVIO_HOST);
	rovio.setDefaultSpeed(5);
	BlobDetector::ColorThresholds red, green, blue;
	red.hueL = 0;
	red.hueH = 20;
	red.satL = 80;
	red.satH = 255;
	red.valL = 80;
	red.valH = 255;
	green.hueL = 60;
	green.hueH = 80;
	green.satL = 80;
	green.satH = 255;
	green.valL = 80;
	green.valH = 255;
	blue.hueL = 100;
	blue.hueH = 115;
	blue.satL = 80;
	blue.satH = 255;
	blue.valL = 80;
	blue.valH = 255;
	
	BlobDetector bd(red, 8, green, 3, blue, 3, 0);
	
	while (true) {
		int index = 0;
		AT;
		vector<BlobDetector::Blob> blobs = bd.findBlobs();
		cout << "Found " << blobs.size() << " blobs" << endl;
		AT;
		
		blobs = bd.filterForLargest(blobs);
		cout << "The largest are: ";
		
		AT;
		for (vector<BlobDetector::Blob>::iterator it = blobs.begin();
			it != blobs.end(); ++it)
		{
			cout << it->color << " (" << it->size << ") ";
		}
		
		AT;
		cout << endl;
		
		map<BlobDetector::BlobCategory, BlobDetector::Blob> cat =
				bd.categorize(blobs);
		cout << "Categorized " << cat.size() << " blobs" << endl;
		
		AT;
		for (map<BlobDetector::BlobCategory, BlobDetector::Blob>::const_iterator it
			= cat.begin(); it != cat.end(); ++it)
		{
			cout << "    Category: " << it->first << ", Color: " << it->second.color
				<< ", Size: " << it->second.size << ", Center: ("
				<< it->second.center.x << ", " << it->second.center.y << ")"
				<< endl;
		}
		
		AT;
		bd.showDebug(cat);
		
		AT;
		if (cat.size() < 3) {
			cout << "Don't have enough points to determine a move." << endl;
			rovio.drive(DIR_STOP);
		} else {
			RobotOrientation ro = getRobotOrientation(
				cat[BlobDetector::ROBOT_LEFT].center,
				cat[BlobDetector::ROBOT_RIGHT].center);
			DUMP(ro.point.x);
			DUMP(ro.point.y);
			DUMP(ro.angle);
			/* // Part 1 : improved	
			DriveCommand command = determineDriveCommand2(ro,
								   cat[BlobDetector::FRUIT].center);
			//			rovio.timeDrive(command.dir,command.speed*100);
			DUMP(command.dir);
			DUMP(command.speed);
			*/
			/* // Part 1 : basic
			DriveDirection command = determineDriveCommand(ro,
								       cat[BlobDetector::FRUIT].center, 45);
			DUMP(command);
			if (command == DIR_FORWARD) {
			  rovio.timeDrive(command, 1000);
			}
			else
			  rovio.drive(command);
			*/
			
			// Part 1 : basic
			DriveDirection command = determineDriveCommand3(ro,
								       cat[BlobDetector::FRUIT].center);
			DUMP(command);
			rovio.timeDrive(command, 1000);
			

			/*// Part2: Face finder
			rovio.saveImage("rovio_img.jpg");
			Mat rovImage = imread("rovio_img.jpg", 1);			
			DriveDirection command;
			command = determineFaceCommand(.2, rovImage);
			std::cout << "Drive direction: " << command << std::endl;
		
			if (command == DIR_FORWARD) {
			  rovio.timeDrive(command, 1000);
			} else {
			  rovio.timeDrive(command, 100);
			}
		
			rovio.drive(DIR_STOP, 500);
			*/

		}
		
		AT;
		if(cvWaitKey(300) == 27) { // exit on 'esc' key
		  break;
		}
	}
	
	cvDestroyAllWindows();
}
